﻿using System;
using System.Net;
using System.Net.Sockets;
using System.Threading;
using System.IO.Ports;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.NetduinoPlus;


namespace NetduinoRobot
{
    public class Program
    {
        public static void Main()
        {
            // the candy lids are controlled by two pins
            // - one pin on, second pin off = move in one direction, 
            // - second pin on, first pin off = move in other direction
            OutputPort candyLidOpen = new OutputPort(Pins.GPIO_PIN_D1, false);
            OutputPort candyLidClosed = new OutputPort(Pins.GPIO_PIN_D2, false);

            double umbrellaSpeedControl = .6666666;

            uint watchdogTimer = 200;
         
            // PWM Pins are 5, 6, 9, 10
            PWM leftDrive = new PWM(Pins.GPIO_PIN_D5);
            PWM rightDrive = new PWM(Pins.GPIO_PIN_D6);
            PWM umbrellaDrive = new PWM(Pins.GPIO_PIN_D9);

            Socket receiveSocket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
            receiveSocket.Bind(new IPEndPoint(IPAddress.Any, 4444));
            byte[] rxData = new byte[10]; // Incoming data buffer

            while (true) /* Main program loop */
            {
                /* Try to receive new data - spend 100uS waiting */
                if (receiveSocket.Poll(100, SelectMode.SelectRead))
                {
                    int rxCount = receiveSocket.Receive(rxData);
                    watchdogTimer = 0;
                }

                if (watchdogTimer < 200)   // Only enable the robot if data was received recently
                {
                    // check the auxbyte - rxData[4] - fifth byte
                    // if the auxByte is set (=255) then this data is for the umbrella and candy lids, else it is for the motors
                    if ((uint)rxData[4] == 255)
                    {
                        // Assuming a CIM Motor used - 900 (full rev) to 2100 (full fwd), 1500 is neutral (use left joystick value rxData[0] - ignore rxData[1]
                        umbrellaDrive.SetPulse(20000, map((uint)rxData[0], 0, 255, (uint)(900+(600-(600*umbrellaSpeedControl))), (uint)(2100-(600-(600*umbrellaSpeedControl)))));


                        if ((int)rxData[2] > 134)
                        {
                            candyLidClosed.Write(true);
                            candyLidOpen.Write(false);
                        }
                        // else if full reverse (2100) open lid
                        else if ((int)rxData[2] < 120)
                        {
                            candyLidClosed.Write(false);
                            candyLidOpen.Write(true);
                        }
                    }
                    else
                    {
                        // this message is for the motors - needs to stay on main thread from umbrella and candy lids

                        // rxData[0] contains y value from left joystick
                        // rxData[1] contains x value from left joystick (ignored in this code)
                        // rxData[2] contains y value from right joystick
                        // rxData[3] contains x value from right joystick (ignored in this code)
                        // rxData[4] contains aux byte

                        // 900 (full rev) to 2100 (full fwd), 1500 is neutral
                        leftDrive.SetPulse(20000, map((uint)rxData[0], 0, 255, 900, 2100));
                        rightDrive.SetPulse(20000, map((uint)rxData[2], 0, 255, 900, 2100));
                    }

                    watchdogTimer++;
                }
                else
                {
                    // Disable the robot
                    leftDrive.SetDutyCycle(0);
                    rightDrive.SetDutyCycle(0);
                    umbrellaDrive.SetDutyCycle(0);
                    candyLidClosed.Write(false);
                    candyLidOpen.Write(false);
                }
            }
        }

        public static uint map(uint x, uint in_min, uint in_max, uint out_min, uint out_max)
        {
            return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
        }
    }
}
